#include "pid_controller.h"

//初始化函数
void set_pid_params(pid_virables *pid_position_controller, double pid_kp, double pid_ki, double pid_kd, double pid_limit_negative, double pid_limit_positive)
{
    pid_position_controller->Kp = pid_kp;
    pid_position_controller->Ki = pid_ki;
    pid_position_controller->Kd = pid_kd;
    pid_position_controller->limit_negative = pid_limit_negative;
    pid_position_controller->limit_positive = pid_limit_positive;
    pid_position_controller->Ki_one_cent = 1 / pid_position_controller->Ki;                                      //
    pid_position_controller->max_ki_set = pid_position_controller->limit_positive / pid_position_controller->Ki; //
    pid_position_controller->min_ki_set = pid_position_controller->limit_negative / pid_position_controller->Ki; //
}

double calculate_pid_output(pid_virables *pid_position_controller, double error_now_come)
{
    //误差递归
    pid_position_controller->error_preview = pid_position_controller->error_last;
    pid_position_controller->error_last = pid_position_controller->error_now;
    pid_position_controller->error_now = error_now_come;
    //计算微分和积分
    pid_position_controller->error_diffs = pid_position_controller->error_now - pid_position_controller->error_last;
    pid_position_controller->error_sum += pid_position_controller->error_now;

    //对微分、积分经行限幅
    if (pid_position_controller->error_sum > pid_position_controller->max_ki_set)
    {
        pid_position_controller->error_sum = pid_position_controller->max_ki_set;
    }
    else if (pid_position_controller->error_sum < pid_position_controller->min_ki_set)
    {
        pid_position_controller->error_sum = pid_position_controller->min_ki_set;
    }
    pid_position_controller->pid_output = pid_position_controller->Kp * pid_position_controller->error_now + pid_position_controller->Ki * pid_position_controller->error_sum + pid_position_controller->Kd * pid_position_controller->error_diffs;
    //对输出限幅
    if (pid_position_controller->pid_output > pid_position_controller->limit_positive)
    {
        pid_position_controller->pid_output = pid_position_controller->limit_positive;
    }
    else if (pid_position_controller->pid_output < pid_position_controller->limit_negative)
    {
        pid_position_controller->pid_output = pid_position_controller->limit_negative;
    }
    //输出
    return pid_position_controller->pid_output;
}
